package com.gitee.wsl.mathematics.matrix.mat3.ext


import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.geometry.EulerOrder
import com.gitee.wsl.mathematics.matrix.mat3.Matrix3
import com.gitee.wsl.mathematics.matrix.mat3.MutableMatrix3
import com.gitee.wsl.mathematics.matrix.mat4.MutableMatrix4
import com.gitee.wsl.mathematics.matrix.mat4.ext.mul33
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.ext.cos
import com.gitee.wsl.unit.angle.ext.cosF
import com.gitee.wsl.unit.angle.ext.sin
import com.gitee.wsl.unit.angle.ext.sinF
import com.gitee.wsl.unit.angle.ext.times
import com.gitee.wsl.unit.angle.toRadians
import com.gitee.wsl.unit.angle.ext.unaryMinus


val<T:Number> Matrix3<T, *, *>.rotation: AngleUnit
                    get() = atan2(m10, m00).toRadians()

fun<T:Number> MutableMatrix3<T, *,*>.setRotate(angle: AngleUnit){
    val cos = angle.cos() + zero
    val sin = angle.sin() + zero
    m00 = cos
    m10 = sin
    m20 = zero

    m01 = -sin
    m11 = cos
    m21 = zero

    m02 = zero
    m12 = zero
    m22 = one
}

fun<T:Number> MutableMatrix3<T,*,*>.setRotateEuler(eulerX: AngleUnit, eulerY: AngleUnit, eulerZ: AngleUnit) {
    val a = eulerX.radian + zero
    val b = eulerY.radian + zero
    val c = eulerZ.radian + zero

    val ci = cos(a)
    val cj = cos(b)
    val ch = cos(c)
    val si = sin(a)
    val sj = sin(b)
    val sh = sin(c)
    val cc = ci * ch
    val cs = ci * sh
    val sc = si * ch
    val ss = si * sh

    m00 = cj * ch
    m01 = sj * sc - cs
    m02 = sj * cc + ss
    m10 = cj * sh
    m11 = sj * ss + cc
    m12 = sj * cs - sc
    m20 = -sj
    m21 = cj * si
    m22 = cj * ci
}

fun<T:Number> MutableMatrix3<T,*,*>.setRotate(axis: Vector3<T,*>, angle: AngleUnit) =
    setRotate(axis.x,axis.y,axis.z,angle)

fun<T:Number> MutableMatrix3<T,*,*>.setRotate(ax: T, ay: T, az: T, angle: AngleUnit) {
    var aX = ax
    var aY = ay
    var aZ = az
    val len = sqrt(aX * aX + aY * aY + aZ * aZ)
    if (!(1f - len).isApproxZero()) {
        val recipLen = 1f / len
        aX *= recipLen
        aY *= recipLen
        aZ *= recipLen
    }

    val radians = angle.radian + zero
    val sin = sin(radians)
    val cos = cos(radians)

    val nc = 1f - cos
    val xy = aX * aY
    val yz = aY * aZ
    val zx = aZ * aX
    val xs = aX * sin
    val ys = aY * sin
    val zs = aZ * sin

    m00 = aX * aX * nc + cos
    m01 = xy * nc - zs
    m02 = zx * nc + ys
    m10 = xy * nc + zs
    m11 = aY * aY * nc + cos
    m12 = yz * nc - xs
    m20 = zx * nc - ys
    m21 = yz * nc + xs
    m22 = aZ * aZ * nc + cos
}

fun<T:Number> MutableMatrix3<T,*,*>.rotateY(angle: AngleUnit) {
    val sin: Float
    val cos: Float
    when (angle) {
        AngleUnit.PI, -AngleUnit.PI -> {
            cos = -1f
            sin = 0f
        }
        AngleUnit.PI * 0.5f, -AngleUnit.PI * 1.5f -> {
            cos = 0f
            sin = 1f
        }
        -AngleUnit.PI * 0.5f, AngleUnit.PI * 1.5f -> {
            cos = 0f
            sin = -1f
        }
        else -> {
            sin = angle.sinF()
            cos = angle.cosF()
        }
    }
    val rm00 = cos
    val rm20 = sin
    val rm02 = -sin
    val rm22 = cos

    // add temporaries for dependent values
    val nm00 = this[0, 0] * rm00 + this[2, 0] * rm02
    val nm01 = this[0, 1] * rm00 + this[2, 1] * rm02
    val nm02 = this[0, 2] * rm00 + this[2, 2] * rm02
    // set non-dependent values directly
    this[2, 0] = this[0, 0] * rm20 + this[2, 0] * rm22
    this[2, 1] = this[0, 1] * rm20 + this[2, 1] * rm22
    this[2, 2] = this[0, 2] * rm20 + this[2, 2] * rm22
    // set other values
    this[0, 0] = nm00
    this[0, 1] = nm01
    this[0, 2] = nm02

}

fun<T:Number> MutableMatrix3<T,*,*>.rotateZ( angle: AngleUnit){
    val sin: Float
    val cos: Float
    when (angle) {
        AngleUnit.PI, -AngleUnit.PI -> {
            cos = -1f
            sin = 0f
        }
        AngleUnit.PI * 0.5f, -AngleUnit.PI * 1.5f -> {
            cos = 0f
            sin = 1f
        }
        -AngleUnit.PI * 0.5f, AngleUnit.PI * 1.5f -> {
            cos = 0f
            sin = -1f
        }
        else -> {
            sin = angle.sinF()
            cos = angle.cosF()
        }
    }
    val rm00 = cos
    val rm10 = -sin
    val rm01 = sin
    val rm11 = cos

    // add temporaries for dependent values
    val nm00 = this[0, 0] * rm00 + this[1, 0] * rm01
    val nm01 = this[0, 1] * rm00 + this[1, 1] * rm01
    val nm02 = this[0, 2] * rm00 + this[1, 2] * rm01
    // set non-dependent values directly
    this[1, 0] = this[0, 0] * rm10 + this[1, 0] * rm11
    this[1, 1] = this[0, 1] * rm10 + this[1, 1] * rm11
    this[1, 2] = this[0, 2] * rm10 + this[1, 2] * rm11
    // set other values
    this[0, 0] = nm00
    this[0, 1] = nm01
    this[0, 2] = nm02

}

/**
 * Inplace operation: Rotates this matrix by the given quaternion rotation. [quaternion] is expected to be a valid
 * rotation quaternion with unit length.
 */
fun<T:Number> MutableMatrix3<T,*,*>.rotate(quaternion: Quaternion<T,*>) {
    val r = quaternion.w
    val i = quaternion.x
    val j = quaternion.y
    val k = quaternion.z

    val t00 = 1 - 2 * (j*j + k*k)
    val t01 = 2 * (i*j - k*r)
    val t02 = 2 * (i*k + j*r)

    val t10 = 2 * (i*j + k*r)
    val t11 = 1 - 2 * (i*i + k*k)
    val t12 = 2 * (j*k - i*r)

    val t20 = 2 * (i*k - j*r)
    val t21 = 2 * (j*k + i*r)
    val t22 = 1 - 2 * (i*i + j*j)

    mul(
        t00, t01, t02,
        t10, t11, t12,
        t20, t21, t22
    )
}

/**
 * Inplace operation: Rotates this matrix around the given [axis] by the given [angle].
 */
fun<T:Number> MutableMatrix3<T,*,*>.rotate(axis: Vector3<T,*>, angle: AngleUnit) {
    val t00: T
    val t01: T
    val t02: T
    val t10: T
    val t11: T
    val t12: T
    val t20: T
    val t21: T
    val t22: T

    val s = angle.sin()+zero
    val c = angle.cos()+zero
    if (axis.x > 0f && axis.y == 0f && axis.z == 0f) {
        // positive x-axis rotation - fast version
        t00 = one; t01 = zero; t02 = zero
        t10 = zero; t11 = c;  t12 = -s
        t20 = zero; t21 = s;  t22 = c

    } else if (axis.x == 0f && axis.y > 0f && axis.z == 0f) {
        // positive y-axis rotation - fast version
        t00 = c;  t01 = zero; t02 = s
        t10 = zero; t11 = one; t12 = zero
        t20 = -s; t21 = zero; t22 = c

    } else if (axis.x == 0f && axis.y == 0f && axis.z > 0f) {
        // positive z-axis rotation - fast version
        t00 = c;  t01 = -s; t02 = zero
        t10 = s;  t11 = c;  t12 = zero
        t20 = zero; t21 = zero; t22 = one

    } else {
        // general case
        val recipLen = 1.0f / axis.length
        val x = axis.x * recipLen
        val y = axis.y * recipLen
        val z = axis.z * recipLen

        val nc = 1.0f - c
        val xy = x * y
        val yz = y * z
        val zx = z * x
        val xs = x * s
        val ys = y * s
        val zs = z * s

        t00 = x * x * nc + c
        t01 = xy * nc - zs
        t02 = zx * nc + ys

        t10 = xy * nc + zs
        t11 = y * y * nc + c
        t12 = yz * nc - xs

        t20 = zx * nc - ys
        t21 = yz * nc + xs
        t22 = z * z * nc + c
    }

     mul(
        t00, t01, t02,
        t10, t11, t12,
        t20, t21, t22
    )
}


/**
 * Inplace operation: Rotates this matrix by the given euler angles.
 */
fun<T:Number> MutableMatrix3<T, *, *>.rotateEulerOrder(eulerX: AngleUnit, eulerY: AngleUnit, eulerZ: AngleUnit, order: EulerOrder = EulerOrder.ZYX) {
    val a = eulerX.cos() + zero
    val b = eulerX.sin() + zero
    val c = eulerY.cos() + zero
    val d = eulerY.sin() + zero
    val e = eulerZ.cos() + zero
    val f = eulerZ.sin() + zero

    val t00: T
    val t01: T
    val t02: T
    val t10: T
    val t11: T
    val t12: T
    val t20: T
    val t21: T
    val t22: T

    when (order) {
        EulerOrder.XYZ -> {
            val ae = a * e; val af = a * f; val be = b * e; val bf = b * f

            t00 = c * e;        t01 = -c * f;       t02 = d
            t10 = af + be * d;  t11 = ae - bf * d;  t12 = - b * c
            t20 = bf - ae * d;  t21 = be + af * d;  t22 = a * c
        }
        EulerOrder.XZY -> {
            val ac = a * c; val ad = a * d; val bc = b * c; val bd = b * d

            t00 = c * e;        t01 = - f;          t02 = d * e
            t10 = ac * f + bd;  t11 = a * e;        t12 = ad * f - bc
            t20 = bc * f - ad;  t21 = b * e;        t22 = bd * f + ac
        }
        EulerOrder.YXZ -> {
            val ce = c * e; val cf = c * f; val de = d * e; val df = d * f

            t00 = ce + df * b;  t01 = de * b - cf;  t02 = a * d
            t10 = a * f;        t11 = a * e;        t12 = -b
            t20 = cf * b - de;  t21 = df + ce * b;  t22 = a * c
        }
        EulerOrder.YZX -> {
            val ac = a * c; val ad = a * d; val bc = b * c; val bd = b * d

            t00 = c * e;        t01 = bd - ac * f;  t02 = bc * f + ad
            t10 = f;            t11 = a * e;        t12 = - b * e
            t20 = - d * e;      t21 = ad * f + bc;  t22 = ac - bd * f
        }
        EulerOrder.ZXY -> {
            val ce = c * e; val cf = c * f; val de = d * e; val df = d * f

            t00 = ce - df * b;  t01 = - a * f;      t02 = de + cf * b
            t10 = cf + de * b;  t11 = a * e;        t12 = df - ce * b
            t20 = - a * d;      t21 = b;            t22 = a * c
        }
        EulerOrder.ZYX -> {
            val ae = a * e; val af = a * f; val be = b * e; val bf = b * f

            t00 = c * e;        t01 = be * d - af;  t02 = ae * d + bf
            t10 = c * f;        t11 = bf * d + ae;  t12 = af * d - be
            t20 = - d;          t21 = b * c;        t22 = a * c
        }
    }

     mul(
        t00, t01, t02,
        t10, t11, t12,
        t20, t21, t22
    )
}

